#include "gimbal.hpp"
#include "bsp_imu.hpp"
#include "can.h"
#include "math.h"
#include "pid.h"
#include "rtthread.h"
#include "tim.h"
#include "shoot.hpp"
#include "referee.hpp"

#define M_PI 3.14159265358979323846

fp32 pitch_pid_angle_param[5]  = {50.0F, 0.0F, 0.0F, 500, 500};      // 拨弹轮 角度环PID 参数
fp32 pitch_pid_speed_param[5]  = {8.0F, 0.5F, 0.1F, 10000, 10000};  				// pitch 速度环PID 参数
fp32 yaw_pid_speed_param[5]    = {100.0F, 6.0F, 5.0F, 20000.0F, 15000.0F};  // yaw轴电机 PID 参数
fp32 yaw_pid_angle_param[5]    = {0.6F, 0.00F, 0.0F, 200, 0}; //0.25F, 0.001F, 0.0F, 400, 100

Gimbal::Gimbal(void) {
    behavior_.pitch_speed_   = 0;
    behavior_.yaw_degree_    = 0;
    behavior_.yaw_speed_     = 0;

    state_ = GIMBAL_STATE_NORMAL;

    /* pitch电机 初始 PID 初始化 */
    shoot_pid_angle_flag_ = false;
		pid_init (&pitch_pid_speed_,PID_POSITION,pitch_pid_speed_param);
		pid_init (&pitch_pid_angle_,PID_POSITION,pitch_pid_angle_param);
    /* Yaw轴电机 初始 PID 初始化 */
		yaw_pid_angle_flag_ = false;
    pid_init(&(yaw_pid_speed_), PID_POSITION, yaw_pid_speed_param);
    pid_init(&yaw_pid_angle_, PID_POSITION, yaw_pid_angle_param);
}
/**
 * @brief 云台初始化函数
 *
 */
int Gimbal::init(void) {
    /* 初始化定时器 */
    timer_ = rt_timer_create("gimbal", timeout, (void *)this, 2, RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_HARD_TIMER);

    set_shoot_speed(0);
    set_yaw_speed(0);
    
		ramp_init(&ramp_pitch_speed_);
    ramp_pitch_speed_.out = 0;
    /* 启动定时器 */
    rt_timer_start(timer_);
		  /* 摄像头电机  初始化 */
		camera_init();

    rt_thread_mdelay(1000);

    pitch_3508_.expect_speed_ = 0;
    yaw_gm6020_.expect_speed_   = 0;

    return 0;
}

/**
 * @brief 云台周期控制函数
 *
 */
void Gimbal::handler(void) {
		/* 摄像头电机  初始化 */
		camera_control();
    motion_solution();
		pitch_limit();
    gimbal_pid_set();
    set_current(pitch_3508_.set_current_, yaw_gm6020_.set_current_, shoot.plunk_.set_current_);
}

/**
 * @brief 云台控制定时器超时函数
 *
 * @param parameter 云台对象
 */
void Gimbal::timeout(void *parameter) {
    Gimbal *t_gimbal = (Gimbal *)parameter;
    t_gimbal->handler();
}
/**
* @brief 图传电机控制
 *
 */
void Gimbal::camera_control()
{
		TIM8->CCR3 = 2400;//图传电机
}
/**
 * @brief 摄像头电机  初始化
 *
 */
void Gimbal::camera_init()
{
//		HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_2);
		HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_3);//图传电机
}
/**
 * @brief 云台角度姿态解算
 *
 */
void Gimbal::motion_solution(void) {


	if(deploy_flag==false)
	{
		pitch_3508_.expect_speed_  = behavior_.pitch_speed_;
		yaw_pid_angle_flag_ = false;
    if (--turning_count > 0) {
        yaw_gm6020_.expect_speed_ = 360;
    } else {
        yaw_gm6020_.expect_degree_ = YAW_ZERO_DEGREE + behavior_.yaw_degree_;
        yaw_gm6020_.expect_speed_  = behavior_.yaw_speed_;
    }
	}
	else{
		  	yaw_pid_angle_flag_ = true;
				if (vt.is_key_pressed_down(KEY_D))
	      {yaw_gm6020_.expect_degree_ -= 2;}
				if (vt.is_key_pressed_down(KEY_A))
	      {yaw_gm6020_.expect_degree_ += 2;}
        yaw_gm6020_.expect_speed_  = behavior_.yaw_speed_;
				behavior_.yaw_degree_=0;
										
				if (vt.is_key_pressed_down(KEY_W))
	      {        pitch_expect_angle = pitch_real_angle + 2000;}
				else if (vt.is_key_pressed_down(KEY_S))
	      {        pitch_expect_angle = pitch_real_angle - 2000;}
				else
				{
					pitch_expect_angle = pitch_real_angle;
				}
    } 
}

/**
 * @brief Pitch 轴角度限位
 *
 */
void Gimbal::pitch_limit(void) {
	if(pitch_3508_.current_>3000&&pitch_3508_.expect_speed_>0)//丝杆电机堵转电流增大
	{pitch_3508_.expect_speed_=0;}
	else if(pitch_3508_.current_<-3000&&pitch_3508_.expect_speed_<0)//丝杆电机堵转电流增大
	{pitch_3508_.expect_speed_=0;}
}

/**
 * @brief 云台电机 PID
 *
 */
void Gimbal::gimbal_pid_set(void) {
    fp32 gyro[3];

    imu.read_gyro(gyro);
    gyro[0] *= (180.0F / M_PI);
    gyro[1] *= (180.0F / M_PI);
    gyro[2] *= (180.0F / M_PI);

    if (yaw_pid_angle_flag_ == true) //角度环控制
		{
       yaw_gm6020_.expect_speed_ = pid_calc(&yaw_pid_angle_, yaw_gm6020_.degree_, yaw_gm6020_.expect_degree_);
    }
		//云台倍率在此处传递，防止浮点数精度缺失
		yaw_gm6020_.set_current_ = pid_calc(&yaw_pid_speed_, gyro[2], yaw_gm6020_.expect_speed_);
			
		ramp_calc(&ramp_pitch_speed_, behavior_.pitch_speed_, PITCH_SPEEDUP_STEP);
		pitch_3508_.expect_speed_=ramp_pitch_speed_.out;
		if(pitch_3508_.expect_speed_>PITCH_MAX_SPEED)
		{pitch_3508_.expect_speed_=PITCH_MAX_SPEED;}
		else if(pitch_3508_.expect_speed_<-PITCH_MAX_SPEED)
		{pitch_3508_.expect_speed_=-PITCH_MAX_SPEED;}
		if(deploy_flag==false)
		{
			pitch_3508_.set_current_= pid_calc(&pitch_pid_speed_, pitch_3508_.speed_, pitch_3508_.expect_speed_);
		}
		else
		{
//			pitch_3508_.set_current_= pid_calc(&pitch_pid_speed_,-gyro[0], pitch_3508_.expect_speed_);
				pitch_3508_.expect_speed_ = pid_calc(&pitch_pid_angle_, pitch_expect_angle, pitch_real_angle);
				pitch_3508_.set_current_  = pid_calc(&pitch_pid_speed_, pitch_3508_.speed_, pitch_3508_.expect_speed_);
		}
    //rt_kprintf("%f\r\n", gyro[2]);
}

/**
 * @brief 状态设定消息队列
 *
 * @param state
 */
void Gimbal::set_state(gimbal_state_t state) {
    state_ = state;
}

/**
 * @brief pitch电机角度数据访问
 *
 * @param shoot_degree 枪管电机角度
 */
void Gimbal::set_shoot_speed(int16_t shoot_degree) {
    behavior_.pitch_speed_ = shoot_degree;
}
/**
 * @brief Yaw轴电机角度数据访问
 *
 * @param yaw_speed Yaw轴电机角度
 */
void Gimbal::set_yaw_speed(int16_t yaw_speed) {
    behavior_.yaw_speed_ = yaw_speed;
}

/**
 * @brief 云台控制接口-调试模式
 *
 * @param shoot_speed Pitch轴角度增量
 * @param yaw_speed Yaw轴角度增量
 */
void Gimbal::manual_mode(int32_t shoot_speed, int16_t yaw_speed) {

	
   set_yaw_speed(yaw_speed);
	 set_shoot_speed(shoot_speed);
	
}

/**
 * @brief 掉头
 *
 */
void Gimbal::turn_around() {
    turning_count =270;
}

/**
 * @brief 读取发射机构电机角度
 *
 * @return int16_t 发射机构电机角度
 */
int16_t Gimbal::get_shoot(void) {
    return pitch_3508_.degree_;
}

/**
 * @brief 读取Yaw轴电机角度
 *
 * @return int16_t Yaw轴电机角度
 */
int16_t Gimbal::get_yaw(void) {
    return yaw_gm6020_.degree_;
}
/**
 * @brief 更新云台电机数据
 *
 * @param ID 电机ID
 * @param Data 电机原始数据
 */
void Gimbal::update(uint8_t id, uint8_t data[8]) {
		  int16_t        stepping;
			int16_t        stepping2;
    if (id == 2) {
        /* Yaw轴 电机数据 */
        // *用原始速度即可，因为控制频率上去之后速度大小和电机反馈的差不多
        yaw_gm6020_.degree_     = (data[0] << 8) | data[1];
        yaw_gm6020_.speed_      = (data[2] << 8) | data[3];
        yaw_gm6020_.current_    = (data[4] << 8) | data[5];
        yaw_gm6020_.temprature_ = data[6];
    }
    if (id == 3) {
        /* pitch轴 电机数据 */
        pitch_3508_.degree_     = (data[0] << 8) | data[1];
        pitch_3508_.speed_      = (data[2] << 8) | data[3];
        pitch_3508_.current_    = (data[4] << 8) | data[5];
        pitch_3508_.temprature_ = data[6];
			
			  stepping2         = pitch_3508_.degree_ - gimbal.pitch_last_angle;
        gimbal.pitch_last_angle = gimbal.pitch_3508_.degree_;

        if (stepping2 < -4096) {
            stepping2 = 8192 + stepping2;
        } else if (stepping2 > 4096) {
            stepping2 = -8192 + stepping2;
        }

        gimbal.pitch_real_angle += stepping / (8192 / 360.0F) / (3591 / 187.0F);
    }
		if (id == 1) {
				shoot.plunk_.degree_  = (data[0] << 8) | data[1];
        shoot.plunk_.speed_   = (data[2] << 8) | data[3];
        shoot.plunk_.current_ = (data[4] << 8) | data[5];
        shoot.plunk_.temprature_ = data[6];
        stepping         = shoot.plunk_.degree_ - shoot.plunk_last_angle;
        shoot.plunk_last_angle = shoot.plunk_.degree_;

        if (stepping < -4096) {
            stepping = 8192 + stepping;
        } else if (stepping > 4096) {
            stepping = -8192 + stepping;
        }

        shoot.plunk_real_angle += stepping / (8192 / 360.0F) / (3591 / 187.0F);
    }
    
}

/**
 * @brief 设置云台电机的电压
 * @param pitch 3508的电流，范围[-16384, 16384]
 * @param Yaw GM6020的电压，范围[-30000, 30000]
 * @param plunk 3508的电流，范围[-16384, 16384]
 */
void Gimbal::set_current(int16_t pitch_motor, int16_t yaw_motor, int16_t plunk_speed) {
    CAN_TxHeaderTypeDef tx_header;
    uint8_t             tx_data[8];
    uint32_t            tx_mailbox;

    /* Yaw轴电机设置电流 */

    /* 配置消息头 */
    tx_header.StdId              = 0x1FFU;
    tx_header.IDE                = CAN_ID_STD;
    tx_header.RTR                = CAN_RTR_DATA;
    tx_header.DLC                = 8;
    tx_header.TransmitGlobalTime = DISABLE;

    /* 计算发送数据 */
    tx_data[0] = plunk_speed >> 8;
    tx_data[1] = plunk_speed;
    tx_data[2] = yaw_motor >> 8;
    tx_data[3] = yaw_motor;
    tx_data[4] = pitch_motor >> 8;
    tx_data[5] = pitch_motor;
    tx_data[6] = 0x00;
    tx_data[7] = 0x00;

    HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data, &tx_mailbox);
}

/**
 * @brief 调试函数
 *
 */
void Gimbal::debug(void) {
    // fp32 gyro[3];
    // imu.read_gyro(gyro);
    // if (!yaw_gm6020_.speed_) {
    //     rt_kprintf("0\n");
    // } else {
    //     rt_kprintf("%f\n", gyro[2] / yaw_gm6020_.speed_);
    // }
//     //rt_kprintf("%d, %d\n", pitch_3508_.current_, pitch_3508_.expect_speed_);
//	rt_kprintf("%d,%d\n",yaw_gm6020_.speed_,pitch_3508_.speed_x);
}
